//
//  BLES6.c
//  BLES
//
//  Created by Khalid on 03/12/2014.
//  Copyright (c) 2014 Khalid. All rights reserved.
//

#include "BLES6.h"



//
///*------------------------------------------------------------------/
// /																	/
// /		Optimise structure boundary by the level set method			/
// /																	/
// /------------------------------------------------------------------*/



//int main(int argc, char *argv[]) //used to check compilation
int initialise(int argc, char *argv) //Working with one function

{
    //printf("\n\n------*---Start of BLES Version 5.4 Program---*------\n");
    
    
    int i;
    
    
    for(i=0;i<100;i++)
    {
        if(argv[i] == '.') {break;} // chop off file extension (if found)
        else if(argv[i] == '\0'){break;}
        else {filename[i] = argv[i];}
    }
    filename[i] = '\0';
    
    // Solve(argv, filename);
    
    char* arg = argv;
    
    // read the input file
    temp = read_input(arg, &inMesh, &numMat, inMat, &levelset, &lsprob, &control, &fixDof,
                      &numCase, &load, &freeDof, &lump_mass, &sw, &acc);
    if(temp==-1){return -1;} // exit on error
    
    if(control.pinfo==3)
    {
        OutNumber(&inMesh, filename);
        OutNodeCoord(&inMesh, filename);
    }
    
    // compute gauss point coords (used in sensitivity smoothing)
    gCoord = malloc(4 * inMesh.NumElem * sizeof(Coord));
    Gauss_Coord(&inMesh, gCoord);
    
    // calculate IN element stiffness matrix (& mass matrix)
    AreaElem = inMesh.h * inMesh.h; // Area of an element
    KE = malloc(numMat*sizeof(double*));
    ME = malloc(numMat*sizeof(double*));
    for(i=0;i<numMat;i++)
    {
        KE[i] = malloc(KE_SIZE*sizeof(double));
        ME[i] = malloc(KE_SIZE*sizeof(double));
        // use 1.0 for thickness, as E & rho already multipled by thickness
        KEMatrix(KE[i], &inMat[i], inMesh.t);	// Element Stiffness Matrix for an IN element
        MEMatrix(ME[i], inMat[i].rho, AreaElem, inMesh.t); // Element Mass Matrix for an IN element
    }
    
    sw = false; 
    
    itt = 0;
    itt0 = 0;
    ReCount = 0;
    cfl = 0.5;
    
    alpha = (double *) malloc(inMesh.NumElem * sizeof(double)); // Array to store element areas
    
    vol_sens=0;
    mass_sens=0;
    zero_sens=0;
    
    num_adj = 0;
    num_sens = 1;
    
    sens_ptr = malloc( (1+lsprob.num) * sizeof(double *) ); // pointer to senstivity arrays for each fucntion (obj + constraints)
    active = malloc(lsprob.num * sizeof(int)); // array for active constraints
    
    // boundary integral variables
    //boundary Bndry;	// boundry discretization
    // Initialise length of AuxNodes & Boundary segment arrays - generous initial size
    Bndry.AuxNodes = malloc(inMesh.NumElem * sizeof(Coord));
    Bndry.Bound = malloc(inMesh.NumElem * sizeof(Bseg));
    Bndry.BsegLen = malloc(sizeof(double)); // initial memory allocation
    Bndry.Bwgt = malloc(sizeof(double)); // initial memory allocation
    Bndry.na_conn = malloc(sizeof(int)); // initial memory allocation
    Bndry.na_conn_ind = malloc( (inMesh.NumNodes+1) * sizeof(int)); // memory allocation
    
    Kg.irn=0; Kg.jcn=0; Kg.A=0; Mg.irn=0; Mg.jcn=0; Mg.A=0;
    
    num_eig = (lsprob.obj == 3) ? 1 : 0; // number of eigenvalues to compute
    comp_eig = (lsprob.obj == 3) ? 1 : 0; // flag to decide if eigenvalues are to be computed
    
    order = (inMesh.NumNodes * NUM_DOF); // full matrix order
    numEnt = inMesh.NumElem * ((NUM_DOF*4)+1)*(NUM_DOF*2); // max number of index entries (symmetry!)
    
    dispLen = order * numCase; // length of disp array (inc fixed dof)
    loadLen = freeDof * numCase; // length of load array (excld fixed dof)
    
    prim_ptr = malloc(numCase * sizeof(double*));
    dual_ptr = malloc(numCase * num_sens * sizeof(double*));
    wgt = malloc(numCase * sizeof(double));
    for(i=0;i<numCase;i++){wgt[i]=1.0;}
    
    // Arrays to store objective & constraint data for each iteration
    obj = malloc(control.maxItt*sizeof(double));
    cnstr = malloc(lsprob.num*control.maxItt*sizeof(double));
    pred = malloc((1+lsprob.num)*control.maxItt*sizeof(double)); // predicted obj & constraint change
    pred_temp = malloc((1+lsprob.num)*sizeof(double)); // temp to feed SLPsubSol
    delcon = malloc(lsprob.num*sizeof(double)); // array to store distance from constraint (constraint violation)
    relax = malloc(lsprob.num*sizeof(double)); // relaxation factor
    for(i=0;i<lsprob.num;i++){relax[i] = 1.0;} // initialize to 1
    
    
    return control.maxItt;
}

void analysis(int itt, double *Py_disp, double *Py_alpha, int *Py_fixDof, double *Py_load )
{
    
    
    //=======================================================================
    
//    do {
        
        //========================== Analysis ends here =========================
        printf("\n\n------------\nStart iteration %i ...",itt);
        /*--------------------------------------------------------------/
         /																/
         /	   Analyse Structure, Solve Primary & Adjoint Equations		/
         /																/
         /--------------------------------------------------------------*/
        
        // Discretize the boundary & compute area ratios for AFG method
        Find_struct(&inMesh, &levelset, &Bndry, alpha, control.aMin);
        Ntot = inMesh.NumNodes + Bndry.NumAux; // total number of nodes
        
        if(control.pinfo > 1 || itt==0)	{
            // output lsf (& alpha) values
            OutPLotShapeVTK2(&inMesh, levelset.lsf, alpha, control.pinfo, itt, filename);
        }
    
        if(control.pinfo > 1) {
            printf("\nTotal nodes (inc. auxillary) = %i",Ntot);
        }
        
        // Assemble global matrices (in triplet form)
        
        // Initialize Index Arrays using dynamic memory allocation
        Kg.ne = numEnt; set_sp_mat(&Kg);
        
        
        
        
        // calculate total structure volume (area) by summing element area array
        ftemp = 0.0;
        for(i=0;i<inMesh.NumElem;i++) {
            ftemp += alpha[i];
        }
        ftemp *= AreaElem;
        printf("\nTotal volume = %12.4e",ftemp);
        if(lsprob.obj==2) // if volume is objective or constraint
        {
            obj[itt] = ftemp; // store value
        }
        else
        {
            for(i=0;i<lsprob.num;i++){
                if(lsprob.con[i].type==1){
                    j = lsprob.num * itt + i; // point to place in cnstr
                    cnstr[j] = ftemp; // store volume as constraint value
                }
            }
        }
        
        
        
        // asemble gloabl stiffness (& maybe mass) matrix
        AFG_Matrix(comp_eig, KE, ME, &Kg, &Mg, &lump_mass, alpha, &inMesh, inMat, control.aMin, control.mMin);
        
        // Solve the FE equation using the MA57 solver
        
        // solve Ku = f (for all cases)
        // remove fixed dof from global matrix
        rem_sp_mat(&Kg, fixDof, 1);
        
        if(lsprob.obj != 3)
        {
            disp = malloc(dispLen * sizeof(double));
            // compute total load vector (in case of self-weight)
            if(sw)
            {
                load_sw = malloc(dispLen * sizeof(double));
                self_weight(&inMesh, inMat, control.aMin, control.mMin, alpha, freeDof, fixDof, numCase, load, load_sw, acc);
                cblas_dcopy(loadLen, load_sw, 1, disp, 1); // copy in load to send to FE_solve
            }
            else
            {
                cblas_dcopy(loadLen, load, 1, disp, 1); // copy in load to send to FE_solve
            }
            FE_Solve(&Kg, fixDof, disp, freeDof, order, numCase);
        }
        
        
        
        // calculate total compliance (if required)
        temp=0;
        if(lsprob.obj==1){temp=1;}
        else {
            for(i=0;i<lsprob.num;i++){ if(lsprob.con[i].type==3){temp=1; break;} }
        }
        if(temp==1) // if compliance is the objective or a constraint
        {
            obj_val = 0.0;
            for(j=0;j<numCase;j++)
            {
                ftemp = 0.0;
                k = freeDof*j;  // point to start of load array for case j
                j2 = order*j;   // point to start of disp array for case j
                for(i=0;i<order;i++) {
                    if(fixDof[i] > -1) {
                        if(sw){ ftemp += disp[j2+i]*load_sw[k+fixDof[i]]; }
                        else{ ftemp += disp[j2+i]*load[k+fixDof[i]]; }
                    }
                }
                printf("\ncompliance for case %i = %12.4e",j+1,ftemp);
                obj_val += ftemp * wgt[j]; // multiply by load case weight
            }
            
            printf("\nTotal compliance = %12.12e",obj_val);
            
            if(lsprob.obj==1){obj[itt]=obj_val;}
            for(i=0;i<lsprob.num;i++){
                if(lsprob.con[i].type==3){
                    j = lsprob.num * itt + i; // point to place in cnstr
                    cnstr[j] = obj_val; // store compliance as constraint value
                }
            }
        }
        
        if(sw){free(load_sw);} // no longer needed
        
        // calculate constraint values and adjoints for displacement constraints
        
        
        // clear memory for K (&M) matrices
        free_sp_mat(&Kg); if(comp_eig==1){free_sp_mat(&Mg);}
        
        if(control.pinfo == 3) {
            // write displacement & mode shape info file
            temp = (lsprob.obj==3) ? 0 : numCase;
            OutDispVTK(&inMesh, temp, disp, 2*num_eig, eig_vecs, itt, filename);
        }
    
    //double foo[5] = { 16, 2, 77, 40, 12071 };
    //double tryPython = 5.0;
    
    //
    //displacement[0] = foo[0];
    //displacement[1] = foo[1];
    
    Py_alpha = alpha;
    Py_disp = disp;
    Py_alpha = alpha;
    Py_fixDof = fixDof;
    Py_load = load;
    
    
    }



int sensitivity(double *Py_Nsens,double *Py_disp, double *Py_alpha)
{
    
    Py_disp = disp;
    Py_alpha = alpha;
    
    //====================================================================
    // Calculate boundary node sensitivities
    Nsens = calloc(Ntot*num_sens,sizeof(double));
    
    // point to correct senstivities etc ...
    
    // setup array of pointers to correct places in disp (for each case)
    if(lsprob.obj != 3 && lsprob.obj != 5)
    {
        for(i=0;i<numCase;i++)
        {
            j = order * i;
            prim_ptr[i] = &disp[j]; // primary variables always displacement
        }
    }
    
    // objective
    temp = 0; // count senstivities (should = num_sens by end)
    temp2 = 0; // count adjoint states (should = num_adj by end)
    if(lsprob.obj==1){
        sens_ptr[0] = &Nsens[temp++]; // compliance sens
        for(i=0;i<numCase;i++){dual_ptr[i*num_sens] = prim_ptr[i];} // compliance is self-adjoint
    }
    
    
    // constraints
    for(i=0;i<lsprob.num;i++)
    {
        j = lsprob.con[i].type;
        if(j==1)
        {
            vol_sens = malloc(Ntot*sizeof(double));
            for(k=0;k<Ntot;k++){ vol_sens[k] = -1.0; } // sensitvity for volume is -1 (geometric constraint)
            sens_ptr[i+1] = vol_sens;
        }
        
        
    }
    
    
    
    // compute boundary shape sensitivies (for compliance or displacement functions)
    AFG_Sens(&inMesh, &Bndry, alpha, inMat, Nsens, prim_ptr, dual_ptr, num_sens, numCase, wgt, gCoord, control.aMin, 0,fact,sw,acc);
    free(disp);
    Py_Nsens = Nsens;
    
    // free required memory
    if(comp_eig==1) { free(eig_vals); free(eig_vecs); }
    if(num_adj>0){ free(adjont); }
    
    if(control.pinfo == 3)	{
        // Write node sensitvity information file (as a boundary mesh)
        OutBoundVTK(&inMesh, &Bndry, (lsprob.num+1), sens_ptr, itt, filename);
    }
    
    // Check convergence criteria
    // calculate difference between current and target constraint
    temp = 1; // check for violated constraints
    j = lsprob.num;
    for(i=0;i<j;i++)
    {
        k = j*itt + i; // pointer for cnstr
        ftemp = cnstr[k]; // current constraint value
        if(lsprob.con[i].type==1)
        {
            delcon[i] = lsprob.con[i].data[0] - ftemp;
            printf("\nVolume constraint violated by %12.4e\n",delcon[i]);
            if(delcon[i]/lsprob.con[i].data[0] < -control.gm){temp=0;}
        }
    }
    
    if(itt > 10+itt0 && temp==1)
    {
        oMax = obj[itt]; // initialize variable for max objective value
        oMin = obj[itt];	// initialize variable for min objective value
        for(i=itt-1;i>itt-10;i--) // work out maximum objective change over last 10 itteration
        {
            ftemp = obj[i];
            oMax = (ftemp > oMax) ? ftemp : oMax; // update maximum objective value
            oMin = (ftemp < oMin) ? ftemp : oMin; // update minimum objective value
        }
        printf("Max obj = %12.4e, Min obj = %12.4e\n",oMax,oMin);
        ftemp = (oMax - oMin) / (oMax + oMin); // normalized max change
        // if max average over last 10 itteration is < gamma then stop!!
        if(ftemp < control.gm) {
            printf("Converged!!! Criteria = %12.4e",ftemp);
            //break;
            return 1;
        }
        else {
            printf("Convergence criteria = %12.4e",ftemp);
        }
    }
    
    // relaxation for constraint change estimates & cfl (on objective)
    if(itt>0)
    {
        // for each constraint
        for(i=0;i<lsprob.num;i++)
        {
            if(active[i] != 0)
            {
                // multiply target constraint change by: actual / predicted from last itt
                j=(1+lsprob.num)*(itt-1) + i+1; // place in predicted (last itt)
                
                ftemp = cnstr[itt*lsprob.num + i];
                ftemp -= cnstr[(itt-1)*lsprob.num + i];  // actual constraint change
                ftemp = pred[j] / ftemp; // relaxation factor
                
                // compute constraint relaxation factors
                if(ftemp > 1.0){relax[i] *= 2.0;} // increase
                if(ftemp < 0.5){relax[i] *= 0.5;} // decrease
                if(relax[i] > 1.0){relax[i] = 1.0;} // upper limit
                else if(relax[i] < 0.125){relax[i] = 0.125;} // lower limit
                
                printf("\nRelaxation factor for %i = %f",i+1,relax[i]);
                delcon[i] *= relax[i];
            }
        }
    }
    Py_Nsens = Nsens;
    return 0;
}




void optimize(void)
{
    
    double *lambda = calloc(1+lsprob.num, sizeof(double)); // lambda values
    
    
    
    //======================================================================================
    // Compute boundary intergral of shape senstivities
    // then use LP sub-problem to obtain the velocity function
    temp = lsprob.num + 1; // number of "functions"
    Vnorm = calloc(Ntot,sizeof(double));		// normal velocity array
    Lbound = malloc(Ntot*temp*sizeof(double));	// boundary intergral data array
    Lbound_nums = malloc(Ntot*sizeof(int)); // global boundary point numbers
    
    // compute variabe weights for boundary intergral
    BsegWgt(&Bndry, &inMesh);
    
    // compute coefficients for discretized boundary integral
    BoundInt(&inMesh, &levelset, &Bndry, temp, sens_ptr, Lbound_nums, &num_Lbound, Lbound);
    
    if(control.pinfo == 3)	{
        // Write boundary integration information file
        OutBoundInt((lsprob.num+1), num_Lbound, Lbound_nums, Lbound, itt, filename);
    }
    
    // realloc & free memory
    Lbound = realloc(Lbound, num_Lbound*temp*sizeof(double));
    Lbound_nums = realloc(Lbound_nums, num_Lbound*sizeof(int));
    
    temp = SLPsubSol4(&inMesh, &levelset, &Bndry, cfl, delcon, lsprob.num, sens_ptr, Lbound, num_Lbound, Lbound_nums, lambda, active, Vnorm, pred_temp, 0, 0, 0, 0, 0, control.pinfo);
    
    
    // copy predicted obj & constraint changes
    k=(1+lsprob.num) * itt;
    for(i=0;i<=lsprob.num;i++)
    {
        pred[k++] = pred_temp[i];
    }
    
    // clear memory
    free(Lbound);
    free(Lbound_nums);
    free(Nsens);
    if(vol_sens){ free(vol_sens); }
    if(mass_sens){ free(mass_sens); }
    if(zero_sens){ free(zero_sens); }
    
    /*--------------------------------------------------------------------------------------/
     /																						/
     /		Calculate Extension Velocities, Gradients and Update the level set function		/
     /																						/
     /--------------------------------------------------------------------------------------*/
    
    // Fast marching method to determine Extension Velocities
    Vext(&inMesh, &levelset, &Bndry, Vnorm);
    
    Grad = calloc(inMesh.NumNodes, sizeof(double)); // array for gradinet info
    
    // Calculate lsf gradients & update
    j2=inMesh.NodeY-1;
    i2=inMesh.NodeX-1;
    for(j=1;j<j2;j++)
    {
        for(i=1;i<i2;i++)
        {
            k = inMesh.Nodes2[i][j]; // read in node number
            if(levelset.active[k]) // If node is active (not fixed and within narrow band)
            {
                if(fabs(Vnorm[k]) > 1.0e-6) // If normal velocity not zero
                {
                    // work out which sign to use for gradient calculation - upwind scheme
                    temp = (Vnorm[k] < -0.0) ? -1 : 1;
                    Grad[k] = GradWENO(i, j, k, levelset.lsf, &inMesh, temp, Vnorm[k], 1.0);
                }
            }
        }
    }
    
    j = inMesh.NumNodes;
    for(k=0;k<j;k++)
    {
        if(levelset.active[k])
        {
            levelset.lsf[k] -=  Grad[k] * Vnorm[k]; // calculate updated lsf value
        }
    }
    
    if(control.pinfo == 3)	{
        // Write Node Gradient & Velocity information file
        OutHJVTK(&inMesh, Vnorm, Grad, itt, filename);
    }
    free(Grad);
    free(Vnorm);
    /*------------------------------------------------------------------/
     /																	/
     /		Re-initalise lsf if mine hit in Narrow Band					/
     /		or 20 iterations have passed since last re-initalization	/
     /																	/
     /------------------------------------------------------------------*/
    
    temp = 0; // flag to see if lsf need to be re-initalised
    if(ReCount < 19)
    {
        for(j=0;j<levelset.numMine;j++) // for all mine nodes
        {
            // if boundary is within 1 grid length of mine then re-initalise
            if( (fabs(levelset.lsf[levelset.mine[j]]) -  inMesh.h) < inMesh.tol )
            {
                temp = 1;
                printf("\n\nMine Hit! ..........Re-initalizing");
                break; // no need to keep hunting for mines
            }
        }
    }
    // set limit of 20 iterations before forcing a re-initialization
    if(temp == 0 && ReCount == 19)
    {
        printf("\n\n20 Iterations have passed! ..........Re-initalizing ");
        temp = 1;
    }
    else { ReCount++; }
    
    if(temp == 1)
    {
        ReCount = 0; // reset reinitialization count
        
        ReInt(&inMesh, &levelset); // reinitialize the lsf
        NarBand(&inMesh, &levelset, control.lband); // re-calculate the bandwidth
        
        printf("\nSigned distance function Re-initialized");
    }
    
    itt++; // next iteration
    
    //while (itt < control.maxItt); // automatically stop after max iterations
    

    
    
}

void output (void)

{
    if(itt == control.maxItt) {
        printf("\nSolution stopped after %i iterations\nObjective Value = %12.4e\n",itt,obj[itt-1]);
    }
    
    // Output the final ls function
    if(control.pinfo == 1) {
        OutPLotShapeVTK2(&inMesh, levelset.lsf, alpha, 1, itt, filename);
    }
    
    // Write Objetcive and Constraint convergence info file
    if(itt == control.maxItt){ itt--; }
    OutConv(itt, &lsprob, obj, cnstr, filename);
    
    // Write eigen frequency info file (is required)
    if(comp_eig==1){ OutFreq(itt, num_eig, save_freq, filename); }
    
    printf("\n\n------*---End of BLES Version 5.4 Program---*------\n\n");
    
}








